#include "RestingTest.h"
#include "TestManager.h"

#include <cassert>
#include <cmath>

using namespace phynet;

const Real RestingTest::PARTICLE_STARTING_SPEED = 20.0f;
const Real RestingTest::PARTICLE_STARTING_ANGLE_GRAD = 45.0f;
const Real RestingTest::PARTICLE_STARTING_DAMPING = 0.990f;

RestingTest::RestingTest(TestManager* testManager)
	: Test(testManager)
	, m_gravityFG(math::Vector2(0.0f, -9.81f))
{
	Init();
}

RestingTest::~RestingTest()
{
}

void RestingTest::Init()
{
	using namespace math;
	using namespace phy;
	m_engine.Init();

	m_nextParticleSpeed = PARTICLE_STARTING_SPEED;
	m_nextParticleAngleGrad = PARTICLE_STARTING_ANGLE_GRAD;
	m_nextParticleDamping = PARTICLE_STARTING_DAMPING;

	Polygon2::Points points;
	points.push_back(Vector2(-15.0f, -5.0f));
	points.push_back(Vector2(-9.0f, -4.0f));
	points.push_back(Vector2(-3.0f, -6.0f));
	points.push_back(Vector2(3.0f, -4.0f));
	points.push_back(Vector2(9.0f, -5.0f));
	points.push_back(Vector2(15.0f, -5.0f));
	points.push_back(Vector2(15.0f, 15.0f));
	points.push_back(Vector2(-15.0f, 15.0f));
	Boundary2D theBoundary;
	theBoundary.m_poly.SetPoints(points);
	theBoundary.m_elasticity = 1.0f;
	m_engine.SetWorldBoundary(theBoundary);

	m_engine.GetWorld().AddForce(&m_gravityFG);
	m_engine.EnableAllCollisions();

	m_pendulumEnabled = false;
}

void RestingTest::Shoot()
{
	const Real angleRad = m_nextParticleAngleGrad * 3.1415926f / 180.0f;
	const math::Vector2 startingVel(m_nextParticleSpeed*cosf(angleRad), m_nextParticleSpeed*sinf(angleRad));
	const int randomSize = (1 + rand()%20);
	const Real particleMass = 0.1f * static_cast<Real>(randomSize);
	const Real particleRadius = 0.1f * sqrt(static_cast<Real>(randomSize));

	phy::Particle2D* newParticle = new phy::Particle2D(particleMass, particleRadius, m_nextParticleDamping, 
		(rand()%50 + 50.0f) / 100.0f, 
		math::Vector2(-14.5f, 0.0f), 
		startingVel);

	m_engine.GetWorld().AddParticle(newParticle);
	m_engine.GetWorld().AddForceOnParticle(&m_gravityFG, newParticle);
}

void RestingTest::ShootTri()
{
	const Real angleRad = m_nextParticleAngleGrad * 3.1415926f / 180.0f;
	const math::Vector2 startingVel(m_nextParticleSpeed*cosf(angleRad), m_nextParticleSpeed*sinf(angleRad));
	phy::Particle2D* newParticle[3];

	for(int i=0; i<3; ++i)
	{
		const int randomSize = (1 + rand()%20);
		const Real particleMass = 0.1f * static_cast<Real>(randomSize);
		const Real particleRadius = 0.1f * sqrt(static_cast<Real>(randomSize));
		newParticle[i] = new phy::Particle2D(particleMass, particleRadius, m_nextParticleDamping, 
			(rand()%50 + 50.0f) / 100.0f, 
			math::Vector2(-14.5f + 1.0f * (i==1), 0.0f + 1.0f * (i==2)), 
			(i==2) ? startingVel*2.0f : math::Vector2::ZERO() );
	}

	m_engine.GetWorld().AddParticles(newParticle, 3);
	m_engine.GetWorld().AddForceOnParticles(&m_gravityFG, newParticle, 3);

	for(int i=0; i<3; ++i)
	{
		phy::ParticleRod2D* rod = new phy::ParticleRod2D;
		rod->Init(newParticle[i], newParticle[(i+1)%3]);
		m_engine.GetWorld().AddLink(rod);
	}
}

void RestingTest::Render(Renderer& renderer) const
{
	typedef math::Vector2 Vector2;
	typedef phy::ParticleWorld2D::Particles Particles;

	const Real speed = m_testManager->GetSlowMotion();

	// instruction
	char buffer[100];
	sprintf(buffer, "Ciao!! e' passato: %.2f  velocita' %.2fX", m_engine.GetWorld().GetTime(), speed);
	renderer.RenderTextLine(buffer);
	sprintf(buffer, "Speed %.2f (-V/+B)", m_nextParticleSpeed);
	renderer.RenderTextLine(buffer);
	sprintf(buffer, "Direction %.2f (-C/+X)", m_nextParticleAngleGrad);
	renderer.RenderTextLine(buffer);
	sprintf(buffer, "Damping %.3f (-K/+L)", m_nextParticleDamping);
	renderer.RenderTextLine(buffer);

	// instruction
	sprintf(buffer, "");
	renderer.RenderTextLine(buffer);
	sprintf(buffer, "Shoot Single Particle (S)");
	renderer.RenderTextLine(buffer);
	sprintf(buffer, "Shoot Tri Particle (D)");
	renderer.RenderTextLine(buffer);
	sprintf(buffer, "");
	renderer.RenderTextLine(buffer);
	sprintf(buffer, "Giga Pendulum %s - (toggle P)", (m_pendulumEnabled ? "enabled" : "disabled"));
	renderer.RenderTextLine(buffer);
	sprintf(buffer, "");
	renderer.RenderTextLine(buffer);

	// cannon
	const Real angleRad = m_nextParticleAngleGrad * 3.1415926f / 180.0f;
	Vector2 direct(cosf(angleRad), sinf(angleRad));
	renderer.RenderLine(Vector2(-14.5f, 0.0f), Vector2(-14.5f, 0.0f) + direct);

	// world
	const Particles& particles = m_engine.GetWorld().GetParticles();
	for (Particles::const_iterator i = particles.begin(); i != particles.end(); ++i)
		renderer.RenderCircle((*i)->GetPosition(), (*i)->GetRadius(), 12);

	//outer boundary
	const math::Polygon2::Points& points = m_engine.GetWorldBoundary().m_poly.GetPoints();
	const size_t numPoints = points.size();
	for (size_t j=0; j<numPoints; ++j)
		renderer.RenderLine(points[j], points[(j+1)%numPoints]);		

	//rods e ropes
	const phy::ParticleWorld2D::Links& links = m_engine.GetWorld().GetLinks();
	for(UInt i=0; i<links.size(); ++i)
	{
		renderer.RenderLine(links[i]->GetEndPoint(0), links[i]->GetEndPoint(1));
	}
}

void RestingTest::Update(float i_elapsedTime)
{
	m_engine.Update(i_elapsedTime);
}

void RestingTest::OnKeyPressed(int keyCode)
{
	if (keyCode == sf::Key::S)
	{
		Shoot();
	}
	else if (keyCode == sf::Key::D)
	{
		ShootTri();
	}
	else if (keyCode == sf::Key::X)
	{
		m_nextParticleAngleGrad += 1.0f;
	}
	else if (keyCode == sf::Key::C)
	{
		m_nextParticleAngleGrad -= 1.0f;
	}
	else if (keyCode == sf::Key::B)
	{
		if(m_nextParticleSpeed < 30.0f)
			m_nextParticleSpeed += 1.0f;
	}
	else if (keyCode == sf::Key::V)
	{
		if(m_nextParticleSpeed >= 2.0f)
			m_nextParticleSpeed -= 1.0f;
	}
	else if (keyCode == sf::Key::L)
	{
		if(math::LessThan(m_nextParticleDamping,1.0f))
			m_nextParticleDamping += 0.001f;
	}
	else if (keyCode == sf::Key::K)
	{
		m_nextParticleDamping -= 0.001f;
	}
	else if (keyCode == sf::Key::P)
	{
		m_pendulumEnabled = !m_pendulumEnabled;
		if(m_pendulumEnabled)
		{
			for(int i=0; i<NUM_PENDULUM_PARTICLES; ++i)
			{
				const int randomSize = (1 + rand()%20);
				const Real particleMass = 0.1f * static_cast<Real>(randomSize);
				const Real particleRadius = 0.1f * sqrt(static_cast<Real>(randomSize));
				m_pendulumParticles[i].Init(particleMass, particleRadius, m_nextParticleDamping, 1.0f, 
					math::Vector2((rand()%80 - 40.0f)/10.0f, 14.0f - 1.0f*i),
					math::Vector2::ZERO());
			}
			m_engine.GetWorld().AddParticles(m_pendulumParticles, NUM_PENDULUM_PARTICLES);
			m_engine.GetWorld().AddForceOnParticles(&m_gravityFG, m_pendulumParticles, NUM_PENDULUM_PARTICLES);
			for(int i=0; i<NUM_PENDULUM_PARTICLES-1; ++i)
			{
				m_pendulumRods[i].Init(&m_pendulumParticles[i], &m_pendulumParticles[i+1]);
				m_engine.GetWorld().AddLink(&m_pendulumRods[i]);
			}
			m_pendulumRodsAncor.Init(&m_pendulumParticles[0], math::Vector2(0.0f, 15.0f));
			m_engine.GetWorld().AddLink(&m_pendulumRodsAncor);
		}
		else
		{
			m_engine.GetWorld().RemoveParticles(m_pendulumParticles, NUM_PENDULUM_PARTICLES);
		}
	}
}
